/*
 *  MatrixHelper.h
 *  NSE_SOLVER
 *
 *  Created by Patrick Zulian on 1/15/11.
 *  Copyright 2011 ICS - USI - Lugano. All rights reserved.
 *
 */

#ifndef NSE_SOLVER_MATRIX_HELPER_H
#define NSE_SOLVER_MATRIX_HELPER_H

#include "Base.h"

namespace nsesolver {
	
	
	template< typename T >
	class MatrixHelper {
	public:
		template< class MatrixType >
		static inline
		const MatrixType pow(const MatrixType &m, const T &exponent)
		{
			//TO DO EFFICENTLY
			MatrixType result(m);
			int e = exponent -1; 
			while( e > 0) {
				result *= m;
				e--;
			}
			return result;
		}
		
		
		template< class MatrixType >
		static inline
		const MatrixType transposed(const MatrixType &m)
		{
			MatrixType result(m.xmax(), m.ymax());
			
			 
			for (int i = 0; i < m.xmax(); i++) {
				for (int j = 0; j < m.ymax(); j++) {
					result(j, i) = m(i, j);
				}
			}
			return result;
		}
		
		
		template< class MatrixType >
		static inline
		void transpose(MatrixType &m)
		{
			NSE_SOLVER_ASSERT( m.ymax() == m.xmax() );
			
			for (int i = 0; i < m.xmax(); i++) {
				for (int j = 0; j < m.ymax(); j++) {
					T temp = static_cast< T > (m(i, j));
					m(i,j) = m(j,i);
					m(j, i) = temp;
				}
			}
		}
		
		template< class MatrixType >
		static inline
		bool equals(const MatrixType &m1, const MatrixType &m2)
		{
			if (&m1 == &m2) {
				return true;
			}
			
			if (m1.ymax() != m2.ymax() || m1.xmax() != m2.xmax()) {
				return false;
			}
			
			for(int i = 0; i < m1.xmax(); i++) {
				for(int j = 0; j < m1.ymax(); j++) {
					if (!Numerics::equalsEps< T >(m1(i,j), m2(i,j))) {
						return false;
					}
				}
			}
			return true;
		}
		
		
		
		template< class MatrixType >
		static inline
		const MatrixType inverted(MatrixType &m)
		{
			NSE_SOLVER_ASSERT( false );
			return m;
		}
		
		
		
		template< class MatrixType >
		static inline
		const MatrixType multiply(MatrixType &result, const MatrixType &m1, const MatrixType &m2)
		{
			NSE_SOLVER_ASSERT( m1.xmax() == m2.ymax() );
			NSE_SOLVER_ASSERT( result.ymax() == m1.ymax() );
			NSE_SOLVER_ASSERT( result.xmax() == m2.xmax() );
			
				for (int j = 0; j < m2.ymax(); j++) {
                    for (int i = 0; i < m1.xmax(); i++) {
			
					
					result(i, j) = 0;
					for (int k = 0; k < m1.ymax(); k++) {
						result(i, j) += m1(i, k) * m2(k, j);
					}
				}
			}
			
			return result;
		}
		
	
		template< class MatrixType >
		static inline
		const MatrixType subtract(const MatrixType &m1, const MatrixType &m2)
		{
			MatrixType result(m1);			
			inPlaceSubtract(result, m2);
			return result;
		}
		
		
		template< class MatrixType >
		static inline
		void inPlaceSubtract(MatrixType &result, const MatrixType &m2)
		{
			NSE_SOLVER_ASSERT( result.xmax() == m2.xmax() );
			NSE_SOLVER_ASSERT( result.ymax() == m2.ymax() );
			
			for (int j = 0; j < result.ymax(); j++) {
                for (int i = 0; i < result.xmax(); i++) {
				
					result(i, j) -= m2(i, j);
				}
			}
			
		}
		
		template< class MatrixType >
		static inline
		const MatrixType sum(const MatrixType &m1, const MatrixType &m2)
		{
			MatrixType result(m1);			
			inPlaceSum(result, m2);
			return result;
		}
		
		
		template< class MatrixType >
		static inline
		void inPlaceSum(MatrixType &result, const MatrixType &m2)
		{
			NSE_SOLVER_ASSERT( result.xmax() == m2.xmax() );
			NSE_SOLVER_ASSERT( result.ymax() == m2.ymax() );
			
            for (int j = 0; j < result.ymax(); j++) {
			for (int i = 0; i < result.xmax(); i++) {
					result(i, j) += m2(i, j);
				}
			}
			
		}
		
		template< class MatrixType >
		static inline
		const MatrixType scale(const MatrixType &m, const T &scaleFactor)
		{
			MatrixType result(m);			
			inPlaceScale(result, scaleFactor);
			return result;
		}
		
		
		template< class MatrixType >
		static inline
		void inPlaceScale(MatrixType &result, const T &scaleFactor)
		{
			
            for (int j = 0; j < result.ymax(); j++) {
			for (int i = 0; i < result.xmax(); i++) {
					result(i, j) *= scaleFactor;
				}
			}
		}
		
		template< class MatrixType >
		static inline
		void each(MatrixType &result, void (*function)(MatrixType &, int, int) )
		{
			
			for (int i = 0; i < result.ymax(); i++) {
				for (int j = 0; j < result.xmax(); j++) {
					function(result, i, j);
				}
			}
		}
		
		template< class MatrixType, class VectorType >
		static inline
		const VectorType matVecMul(VectorType & result, const MatrixType &m, const VectorType &v)
		{
			//Check if it is an equal size vector or it this is considered as an homogenous coordinates matrix
			NSE_SOLVER_ASSERT( v.size() == m.xmax() || (v.size() == m.xmax() - 1) );
			NSE_SOLVER_ASSERT( result.size() == m.ymax() || (result.size() == m.ymax() - 1) );

			
			for(int i = 0; i < result.size(); i++) {
				result[i] = 0;				
				for(int j = 0; j < v.size(); j++) {
					result[i] += v[j] * m(i, j);
				}
			}
			
			if (result.size() == m.ymax() && v.size() == m.xmax()) {
				return result;
			}
			
			// Handle homogeneous coordinate transformation
			for (int i = 0; i < result.size(); i++) {
				result[i] += m(i, m.xmax() -1);
			}
			
			T scale = 0;
			for (int j = v.size() -1; j < m.xmax(); j++) {
				scale += m(m.ymax() -1, j);
			}
			
			if(scale == 1 || scale == 0)
				return result;
			
			return result/scale;
		}
		
		
		template< class MatrixType >
		static inline
		void Identity(MatrixType & result)
		{
			each< MatrixType >(result, identityAux);
		}
		
		
		
	private:
		template< class MatrixType >
		static inline
		void identityAux(MatrixType &result, int i, int j)
		{
			result(i, j) = (i == j);
		}
	};
	
	
}


#endif
